import Glibc
class Timer {
    private let CLOCK_REALTIME = 0
    private var start_timespec = timespec()
    private var end_timespec = timespec()
    private var time_spec = timespec()
    func start() {
        clock_gettime(Int32(CLOCK_REALTIME),&start_timespec)
    }
    
    func stop() -> Double {
        clock_gettime(Int32(CLOCK_REALTIME),&end_timespec)
        let start_time = Double(start_timespec.tv_sec * 1_000_000 + start_timespec.tv_nsec / 1_000)
        let end_time = Double(end_timespec.tv_sec * 1_000_000 + end_timespec.tv_nsec / 1_000)
        let time = end_time - start_time
        return time / 1_000
    }
    func getTime() -> Double {
        clock_gettime(Int32(CLOCK_REALTIME),&time_spec)
        return Double(time_spec.tv_sec * 1_000_000 + time_spec.tv_nsec / 1_000)
    }
}

let timer = Timer()
var global = 0.0

let AG_CONST: Double = 0.6072529350
    
func FIXED(_ X: Double) -> Double {
    return X * 65536.0
}
    
func FLOAT(_ X: Double) -> Double {
    return X / 65536.0
}
    
func DEG2RAD(_ X: Double) -> Double {
    return 0.017453 * (X)
}
    
let Angles: [Double] = [
    FIXED(45.0), FIXED(26.565), FIXED(14.0362), FIXED(7.12502),
    FIXED(3.57633), FIXED(1.78991), FIXED(0.895174), FIXED(0.447614),
    FIXED(0.223811), FIXED(0.111906), FIXED(0.055953),
    FIXED(0.027977)
]
    
let Target: Double = 28.027
    
func cordicsincos(_ Target: Double) -> Double {
    var X: Double
    var Y: Double
    var TargetAngle: Double
    var CurrAngle: Double
    
    X = FIXED(AG_CONST)
    Y = 0
    TargetAngle = FIXED(Target)
    CurrAngle = 0
    for Step in 0..<12 {
        var NewX: Double
        if TargetAngle > CurrAngle {
            NewX = X - (Y / Double(1 << Step))
            Y = (X / Double(1 << Step)) + Y
            X = NewX
            CurrAngle += Angles[Step]
        } else {
            NewX = X + (Y / Double(1 << Step))
            Y = -(X / Double(1 << Step)) + Y
            X = NewX
            CurrAngle -= Angles[Step]
        }
    }
    return FLOAT(X) * FLOAT(Y)
}

func RunCordic() -> Int {
    var res:Double = 0.0
    let input = 150000
    timer.start()
    for _ in 0..<input {
        res += cordicsincos(Target)
    }
    let time = timer.stop()
    global = res;
    // XCTAssertEqual(res, 0.4145028187500179)
    print("Numerical Calculation - RunCordic:\t"+String(time)+"\tms");
    return Int(time)
}

_ = RunCordic()
print("Cordic Is End , global value is \(global)")
// print(RunCordic())